#include "nav2_util/node_utils.hpp"
#include <cmath>
#include <memory>
#include <string>

#include "nav2_core/exceptions.hpp"
#include "nav2_custom_planner/nav2_custom_planner.hpp"

namespace nav2_custom_planner{
        void CustomPlanner::configure(
            const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent,
            std::string name,
            std::shared_ptr<tf2_ros::Buffer> tf,
            std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros
        ){
            tf_ = tf;
            node_ = parent.lock();
            name_ = name;
            costmap_ = costmap_ros->getCostmap();
            global_frame_ = costmap_ros->getGlobalFrameID();
            
            // 参数初始化
            nav2_util::declare_parameter_if_not_declared(
                node_, name_+".interpolation_resolution",
                rclcpp::ParameterValue(0.1)
            );
            node_->get_parameter(name + ".interpolation_resolution", 
                interpolation_resolution_);
        }

        void CustomPlanner::cleanup(){
            RCLCPP_INFO(node_->get_logger(), "正在清理类型为CustomPlanner的插件%s", name_.c_str());
        }

        void CustomPlanner::activate(){
            RCLCPP_INFO(node_->get_logger(), "正在激活类型为CustomPlanner的插件%s", name_.c_str());
        }

        void CustomPlanner::deactivate(){
            RCLCPP_INFO(node_->get_logger(), "正在停用类型为CustomPlanner的插件%s", name_.c_str());
        }

        nav_msgs::msg::Path CustomPlanner::createPlan(const geometry_msgs::msg::PoseStamped &start,
        const geometry_msgs::msg::PoseStamped &goal){
            nav_msgs::msg::Path global_path;
            //进行规划
            // 1.初始化 global_path
            global_path.poses.clear();
            global_path.header.stamp = node_->now();
            global_path.header.frame_id = global_frame_;
            //2.检查目标和起点是否在全局坐标系中
            if(start.header.frame_id != global_frame_){
                RCLCPP_INFO(node_->get_logger(), "规划器仅接受来自%s坐标系的起始位置",
                global_frame_.c_str());
                return global_path;
            }
            if(goal.header.frame_id != global_frame_){
                RCLCPP_INFO(node_->get_logger(), "规划器仅接受来自%s坐标系的目标位置",
                global_frame_.c_str());
                return global_path;
            }
            //3.计算当前插值分辨率interpolation_resolution_下的循环次数和步进值
            int total_number_of_loop = std::hypot(
                goal.pose.position.x - start.pose.position.x,
                goal.pose.position.y - start.pose.position.y)/interpolation_resolution_;
            double x_increment = (goal.pose.position.x - start.pose.position.x)/total_number_of_loop;
            double y_increment = (goal.pose.position.y - start.pose.position.y)/total_number_of_loop;
            
            //4.生成路径
            for (int i = 0;i < total_number_of_loop;++i){
                geometry_msgs::msg::PoseStamped pose;// 生成一个点
                pose.pose.position.x = start.pose.position.x + x_increment * i;
                pose.pose.position.y = start.pose.position.y + y_increment * i;
                pose.pose.position.z = 0;
                pose.header.stamp = node_->now();
                pose.header.frame_id = global_frame_;
                // 将该点放到路径中
                global_path.poses.push_back(pose);
            }
            //5.使用costmap检查该条路径是否经过障碍物
            for(geometry_msgs::msg::PoseStamped pose : global_path.poses){
                unsigned int mx, my;
                //将点的坐标转换为栅格坐标
                if (costmap_->worldToMap(pose.pose.position.x,pose.pose.position.y, mx, my))
                {
                    unsigned char cost = costmap_->getCost(mx, my);//获取对应栅格的代价值
                    // 如存在致命障碍则抛出异常
                    if (cost == nav2_costmap_2d::LETHAL_OBSTACLE)
                    {
                        RCLCPP_INFO(node_->get_logger(), "在(%f, %f)处检测到障碍物， 规划失败",
                                    pose.pose.position.x, pose.pose.position.y);
                        throw nav2_core::PlannerException("无法创建目标规划:"+
                            std::to_string(goal.pose.position.x)+","+
                            std::to_string(goal.pose.position.y));
                    }
                }
            }
            // 6.收尾， 建目标点作为路径的最后一个点并返回路径
            geometry_msgs::msg::PoseStamped goal_pose = goal;
            goal_pose.header.stamp = node_->now();
            goal_pose.header.frame_id = global_frame_;
            global_path.poses.push_back(goal_pose);
            return global_path;
        }
}// namespace nav2_custom_planner

#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(nav2_custom_planner::CustomPlanner,
                        nav2_core::GlobalPlanner)